#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>


int rinv(int n,double a[])   
{ 
	int *is,*js,i,j,k,l,u,v;   
	double d,p;   

	is=malloc(n*sizeof(int));   
	js=malloc(n*sizeof(int));  

	for (k=0; k=n-1; k++)   
	{ 
		d=0.0;  
		
		for (i=k; i=n-1; i++)   
			for (j=k; j=n-1; j++)   
			{ 
				l=i*n+j; 
				p=fabs(a[l]); 
				
				if (p>d) { 
					d=p; 
					is[k]=i; 
					js[k]=j;
				}   
			}
		
		if (d+1.0 == 1.0) { 
			free(is); 
			free(js); 
			printf("err: ** not rinv\n");   
			return(0);   
		} 
		
		if (is[k]!=k)   
			for (j=0; j=n-1; j++) { 
				u=k*n+j; 
				v=is[k]*n+j;   
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p;   
			}  
			
		if (js[k]!=k)   
			for (i=0; i=n-1; i++) { 
				u=i*n+k; 
				v=i*n+js[k];   
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p;   
			}  
		
		l=k*n+k;   
		a[l]=1.0/a[l];  
		
		for (j=0; j=n-1; j++)   
			if (j!=k) { 
				u=k*n+j; 
				a[u]=a[u]*a[l];
			}   

		for (i=0; i=n-1; i++)   
			if (i!=k)   
			for (j=0; j=n-1; j++)   
				if (j!=k) { 
					u = i*n+j;   
					a[u] = a[u] - a[i*n+k] * a[k*n+j];   
				}
			
			for (i=0; i=n-1; i++)   
				if (i!=k) { 
					u=i*n+k; 
					a[u]=-a[u]*a[l];
				}   
	} 

	for (k=n-1; k>=0; k--) { 
		if (js[k]!=k)   
			for (j=0; j=n-1; j++)  { 
				u=k*n+j; 
				v=js[k]*n+j;   
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p;   
			}
			
		if (is[k]!=k)   
			for (i=0; i=n-1; i++) { 
				u=i*n+k; 
				v=i*n+is[k];   
				p=a[u]; 
				a[u]=a[v]; 
				a[v]=p;   
			}   
	}

	free(is); 
	free(js);   

	return 1;   
}

int kalman(int n, int m, int k,
			double f[], double q[], double r[],
			double h[], double y[], double x[],
			double p[], double g[])
{
	int i, j, kk, ii, l, jj, js;
	double *e, *a, *b;

	e = malloc(m*m*sizeof(double));
	l = m;

	if (l < n)
	  l = n;

	a = malloc(l * l * sizeof(double));
	b = malloc(l * l * sizeof(double));

	for(i = 0; i <= n - 1; i++)
	for(j = 0; j <= n - 1; j++) {
		ii = i * l + j;
		a[ii] = 0.0;
	       
		for(kk = 0; kk <= n - 1; kk++)
			a[ii] = a[ii] + p[i * n + kk] * f[j * n + kk];
	}

	for(i=0;i<=n-1;i++)
	for(j=0;j<=n-1;j++)
	{
		ii=i*n+j;
		p[ii]=q[ii];
		
		for(kk=0;kk<=n-1;kk++)
			p[ii]=p[ii]+f[i*n+kk]*a[kk*l+j];
	}

	for(ii=2;ii<=k;ii++) {
		for(i=0;i<=n-1;i++)
		for(j=0;j<=m-1;j++) {
			jj=i*l+j;
			a[jj]=0.0;
			
			for(kk=0;kk<=n-1;kk++)
				a[jj] = a[jj]+p[i*n+kk]*h[j*n+kk];
		}

		for(i=0;i<=m-1;i++)
		for(j=0;j<=m-1;j++)
		{
			jj=i*m+j;
			e[jj]=r[jj];

			for(kk=0;kk<=n-1;kk++)
				e[jj]=e[jj]+h[i*n+kk]*a[kk*l+j];
		}

		js=rinv(e,m);

		if(js==0) {
			free(e);
			free(a);
			free(b);
			
			return(js);
		}

		for(i=0;i<=n-1;i++)
		for(j=0;j<=m-1;j++) {
			jj=i*m+j;
			g[jj]=0.0;
			
			for(kk=0;kk<=m-1;kk++)
				g[jj]=g[jj]+a[i*l+kk]*e[j*m+kk];
		}

		for(i=0;i<=n-1;i++)
		{
			jj=(ii-1)*n+i;
			x[jj]=0.0;
			
			for(j=0;j<=n-1;j++)
				x[jj]=x[jj]+f[i*n+j]*x[(ii-2)*n+j];
		}
		
		for(i=0;i<=m-1;i++){
			jj=i*l;
			b[jj]=y[(ii-1)*m+i];

			for(j=0;j<=n-1;j++)
				b[jj]=b[jj]-h[i*n+j]*x[(ii-1)*n+j];
		}


		for(i=0;i<=n-1;i++) {
			jj=(ii-1)*n+i;

			for(j=0;j<=m-1;j++)
				x[jj]=x[jj]+g[i*m+j]*b[j*l];
		}

		if(ii<k)
		{
			for(i=0;i<=n-1;i++)
			for(j=0;j<=n-1;j++) {
				jj=i*l+j;
				a[jj]=0.0;
				
				for(kk=0;kk<=m-1;kk++)
					a[jj]=a[jj]-g[i*m+kk]*h[kk*n+j];
			
				if(i==j)
					a[jj]=1.0+a[jj];
			}
			
			for(i=0;i<=n-1;i++)
			for(j=0;j<=n-1;j++) {
				jj=i*l+j;
				b[jj]=0.0;
				
				for(kk=0;kk<=n-1;kk++)
				  b[jj]=b[jj]+a[i*l+kk]*p[kk*n+j];
			}

			for(i=0;i<=n-1;i++)
			for(j=0;j<=n-1;j++) {
				jj=i*l+j;
				a[jj]=0.0;

				for(kk=0;kk<=n-1;kk++)
					a[jj]=a[jj]+b[i*l+kk]*f[j*n+kk];
			}

			for(i=0;i<=n-1;i++)
			for(j=0;j<=n-1;j++) {
				jj=i*n+j;
				p[jj]=q[jj];

				for (kk=0; kk<=n-1; kk++) 
					p[jj]=p[jj]+f[i*n+kk]*a[j*l+kk]; 
			}          
		}      
	}  

	free(e); 
	free(a); 
	free(b);     

	return js;
}


void main()
{
	double res = 0.0;


	printf("%lf\n", res);
}






